{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "# nuScenes CAN bus tutorial\n",
    "This page describes how to use the nuScenes CAN bus expansion data.\n",
    "The CAN bus is a vehicle bus over which information such as position, velocity, acceleration, steering, lights, battery and many more are submitted.\n",
    "We recommend you start by reading the [README](https://github.com/nutonomy/nuscenes-devkit/tree/master/python-sdk/nuscenes/can_bus/README.md)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setup\n",
    "To install the can bus expansion, please download the files from https://www.nuscenes.org/download and copy the files into your nuScenes can folder, e.g. `/data/sets/nuscenes/can_bus`. You will also need to update your `nuscenes-devkit`."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Initialization\n",
    "To initialize the can bus API, run the following:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from nuscenes.can_bus.can_bus_api import NuScenesCanBus\n",
    "nusc_can = NuScenesCanBus(dataroot='/data/sets/nuscenes')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Overview\n",
    "Let us get an overview of all the CAN bus messages and some basic statistics (min, max, mean, stdev, etc.). We will pick an arbitrary scene for that."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "scene_name = 'scene-0001'\n",
    "nusc_can.print_all_message_stats(scene_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualization\n",
    "Next we plot the values in a CAN bus message over time. \n",
    "\n",
    "As an example let us pick the steering angle feedback message and the key called \"value\" as described in the [README](https://github.com/nutonomy/nuscenes-devkit/tree/master/python-sdk/nuscenes/can_bus/README.md). The plot below shows the steering angle. It seems like the scene starts with a strong left turn and then continues more or less straight."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "message_name = 'steeranglefeedback'\n",
    "key_name = 'value'\n",
    "nusc_can.plot_message_data(scene_name, message_name, key_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the data we want to plot is multi-dimensional, we need to provide an additional argument to select the dimension. Here we plot the acceleration along the lateral dimension (y-axis). We can see that initially this acceleration is higher."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "message_name = 'pose'\n",
    "key_name = 'accel'\n",
    "nusc_can.plot_message_data(scene_name, message_name, key_name, dimension=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can also retrieve the raw data and compare the wheel speeds to the vehicle speeds. Here we convert the wheel speed from rounds per minute to m/s and the vehicle speed from km/h to m/s. We can see that there is a small offset between the speeds."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "# Retrieve raw data.\n",
    "wheel_speed = nusc_can.get_messages(scene_name, 'zoe_veh_info')\n",
    "wheel_speed = np.array([(m['utime'], m['FL_wheel_speed']) for m in wheel_speed])\n",
    "\n",
    "veh_speed = nusc_can.get_messages(scene_name, 'vehicle_monitor')\n",
    "veh_speed = np.array([(m['utime'], m['vehicle_speed']) for m in veh_speed])\n",
    "\n",
    "# Convert to m/s.\n",
    "radius = 0.305  # Known Zoe wheel radius in meters.\n",
    "circumference = 2 * np.pi * radius\n",
    "wheel_speed[:, 1] *= circumference / 60\n",
    "veh_speed[:, 1] *= 1 / 3.6\n",
    "\n",
    "# Normalize time.\n",
    "wheel_speed[:, 0] = (wheel_speed[:, 0] - wheel_speed[0, 0]) / 1e6\n",
    "veh_speed[:, 0] = (veh_speed[:, 0] - veh_speed[0, 0]) / 1e6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(wheel_speed[:, 0], wheel_speed[:, 1])\n",
    "plt.plot(veh_speed[:, 0], veh_speed[:, 1])\n",
    "plt.xlabel('Time in s')\n",
    "plt.ylabel('Speed in m/s')\n",
    "plt.legend(['Wheel speed', 'Vehicle speed']);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now let us render the baseline route for this scene. The blue line below shows the baseline route extended by 50m beyond the start and end of the scene. The orange line indicates the ego vehicle pose. To differentiate the start and end point we highlight the start with a red cross. We can see that there is a slight deviation of the actual poses from the route."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nusc_can.plot_baseline_route(scene_name)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Error handling\n",
    "Please note that some scenes are not well aligned with the baseline route. This can be due to diversions or because the human driver was not following a route. We compute all misaligned routes by checking if each ego pose has a baseline route within 5m."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(nusc_can.list_misaligned_routes())"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Furthermore a small number of scenes have no CAN bus data at all. These can therefore not be used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(nusc_can.can_blacklist)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.7"
  },
  "pycharm": {
   "stem_cell": {
    "cell_type": "raw",
    "metadata": {
     "collapsed": false
    },
    "source": []
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
